import pybullet as p
import pybullet_data

from pyrosim import pyrosim


class WORLD():
    
    def __init__(self, position):
        
        self.__position = position
        # self.__create_target()
        
    def __create_target(self) -> None:
        
        """创建目标物体"""
        
        pyrosim.Start_SDF(f"models/target.sdf")
        pyrosim.Send_Cube(name="Target", pos=self.__position, size=[1, 1, 1])
        pyrosim.End()
        
    def load_world(self) -> None: 
        
        """加载外部环境"""   
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        p.loadURDF("plane.urdf", useFixedBase=1)
        p.loadURDF(f"models/target.urdf", basePosition=self.__position, useFixedBase=1)
